#include "cluster.h"
void clusterClosest(FindRobots& finder, int color, bool run)
{
	const double offset = 3.14/4;
    std::vector<Robot> robots = finder.getRobotPositions();
    double rad = -1;
    double angle = -1;
    Motor motor;
    Bumper bumpStat;
    do 
    {

        if(bumpStat.isFrontTouching())
        {
            motor.stop();
            break;
        }

        robots = finder.getRobotPositions();
        //cout << robots.size() << " ";
	rad = -1;
        for(int i = 0; i < robots.size(); i++)
        {
            if(rad < 0 || robots[i].position.radius_cm < rad)
            {
                rad   = robots[i].position.radius_cm;
                angle = robots[i].position.angle_rad;
            }
        }
        if(robots.size() > 0)
        {
            //cout << "Rad " << rad << " Angle " << angle << endl;
            if(angle > offset)
            {
                //cout << "Crawl left\n";
                motor.leftWheelStop();
                motor.rightWheelGoOffset(5);
            }
            if(angle < -offset)
            {
                //cout << "Crawl right\n";
                motor.rightWheelStop();
                motor.leftWheelGoOffset(5);
            }
            if(angle > -offset && angle < offset)
            {
                //cout << "Crawl forward\n";
                motor.goOffset(5);
            }

            struct timespec tim, tim2;
            tim.tv_sec = 0;
            tim.tv_nsec = 500000000;

//            nanosleep(&tim , &tim2);
//            motor.stop();
//            nanosleep(&tim , &tim2);
        }
        else {
            motor.stop();
        }
    } while(run);
}
void clusterFurthest(FindRobots& finder, int color, bool run)
{
	const double offset = 3.14/4;
	std::vector<Robot> robots = finder.getRobotPositions();
    	double rad = -1;
	double angle = -1;
	Motor motor;
	Bumper bumpStat;
	do 
	{

		if(bumpStat.isFrontTouching())
		{
		    motor.stop();
		    break;
		}

		robots = finder.getRobotPositions();
		//cout << robots.size() << " ";
		rad = -1;
		for(int i = 0; i < robots.size(); i++)
		{
		    if(rad < 0 || robots[i].position.radius_cm > rad)
		    {
			rad   = robots[i].position.radius_cm;
			angle = robots[i].position.angle_rad;
		    }
		}
		if(robots.size() > 0)
		{
		    //cout << "Rad " << rad << " Angle " << angle << endl;
		    if(angle > offset)
		    {
			//cout << "Crawl left\n";
			motor.leftWheelStop();
			motor.rightWheelGoOffset(5);
		    }
		    if(angle < -offset)
		    {
			//cout << "Crawl right\n";
			motor.rightWheelStop();
			motor.leftWheelGoOffset(5);
		    }
		    if(angle > -offset && angle < offset)
		    {
			//cout << "Crawl forward\n";
			motor.goOffset(5);
		    }

		    struct timespec tim, tim2;
		    tim.tv_sec = 0;
		    tim.tv_nsec = 500000000;

		//            nanosleep(&tim , &tim2);
		//            motor.stop();
		//            nanosleep(&tim , &tim2);
		}
		else {
		    motor.stop();
		}
	} while(run);
}
void clusterDirection(FindRobots& finder, const double anglePrime, int color, bool run)
{
	
}
void clusterTrack(FindRobots& finder, double& rad, double& angle, Motor &motor, Bumper &bumpStat, int color)
{
	const double offset = 3.14/4;
	std::vector<Robot> robots = finder.getRobotPositions();

    	//double rad = -1;
	//double angle = -1;
	double cpd, tcpd;//Temporary Combined Percent Difference
	double trad, tangle;//Temporary radius and angle 
//	Motor motor;
//	Bumper bumpStat;
//	do 
//	{

		if(bumpStat.isFrontTouching())
		{
		    motor.stop();
		    return;
		}

		robots = finder.getRobotPositions();
		//cout << robots.size() << " ";
		cpd = -1;
		bool foundMyColor = false;
		double x1, y1, xt, yt;
		x1 = rad*sin(-angle);
		y1 = rad*cos(angle);
		for(int i = 0; i < robots.size(); i++)
		{
			if(color == LED::ANY || color == LED::colorStringToInt(robots[i].label))
			{
				foundMyColor = true;
				/*if(fabs(angle - robots[i].position.angle_rad) < 3.14)
				{
					tcpd = fabs(rad - robots[i].position.radius_cm)/rad +
					fabs(angle - robots[i].position.angle_rad)/3.14;//angle ;
				}
				else
				{
					tcpd = fabs(rad - robots[i].position.radius_cm)/rad +
					(fabs(angle + robots[i].position.angle_rad))/3.14;//angle ;
				}*/
				x1 = robots[i].position.radius_cm*sin(-robots[i].position.angle_rad);
				y1 = robots[i].position.radius_cm*cos(robots[i].position.angle_rad);
				tcpd = sqrt(pow(x1-xt, 2) + pow(y1 - y2, 2));
				if(cpd < 0 || tcpd < cpd)
				{
					trad   = robots[i].position.radius_cm;
					tangle = robots[i].position.angle_rad;
					cpd = tcpd;
				}
			}
		}
		if(foundMyColor)
		{
			rad = trad;
			angle = tangle;
			//cout << "Rad " << rad << " Angle " << angle << endl;
			if(angle > offset)
			{
				//cout << "Crawl left\n";
				motor.leftWheelStop();
				motor.rightWheelGoOffset(5);
			}
			if(angle < -offset)
			{
				//cout << "Crawl right\n";
				motor.rightWheelStop();
				motor.leftWheelGoOffset(5);
			}
			if(angle > -offset && angle < offset)
			{
				//cout << "Crawl forward\n";
				motor.goOffset(5);
			}

//			struct timespec tim, tim2;
//			tim.tv_sec = 0;
	//		tim.tv_nsec = 500000000;

		//            nanosleep(&tim , &tim2);
		//            motor.stop();
		//            nanosleep(&tim , &tim2);
		}
		else 
		{
				motor.rightWheelReverseOffset(5);
				motor.leftWheelGoOffset(5);
		}
//	} while(run);
}
